#ifndef FAKE_LASER_H
#define FAKE_LASER_H

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <agv_msgs/msg/pose_with_confidence.hpp>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include "ros_utils/TransformHandler.h"
#include "fake_laser/LaserGenerator.h"
#include "occupancyMap/OccupancyGridMap.h"

class FakeLaser : public rclcpp::Node
{
public:
  FakeLaser();
  bool init();

private:
  void mapCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
  void updateLaserPose();
  void generateFakeLaser();

  // Transform handler
  std::shared_ptr<utils::TransformHandler> tf_;

  // Topics
  std::string map_topic_;
  std::string laser_topic_;
  std::string pose_topic_;

  // Frame IDs
  std::string baselink_frame_;
  std::string odom_frame_;
  std::string map_frame_;
  std::string laser_frame_;

  // Laser parameters
  double laser_range_min_;
  double laser_range_max_;
  double laser_offset_x_;
  double laser_offset_y_;
  double laser_offset_z_;

  // Subscribers
  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr sub_map_;

  // Publishers
  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pub_laser_;
  rclcpp::Publisher<agv_msgs::msg::PoseWithConfidence>::SharedPtr pub_pose_;

  // Timers
  rclcpp::TimerBase::SharedPtr pose_update_timer_;
  rclcpp::TimerBase::SharedPtr pub_laser_timer_;

  // Helper classes
  OccupancyGridMap occupancy_map_;
  LaserGenerator laser_generator_;
};

#endif // FAKE_LASER_H